/**
* @file CliffData.h
* @author
* @brief 探地传感器数据类型
* @version 1.0
* @date 2020-07-23
*
* @copyright Copyright (c) 2020
*
*/

#pragma once

#include <cstdint>
#include "data_struct/robot/Pose.h"
#include "lib/Time.h"

namespace behavior_controller
{
enum CliffId
{
    NO_CLIFF = 0,
    LEFT_FRON = 1,  //左前cliff
    LEFT_MIDD = 2,  //左中cliff
    LEFT_BACK = 4,  //左后cliff
    RIGH_BACK = 8,  //右后cliff
    RIGH_MIDD = 16, //右中cliff
    RIGH_FRON = 32  //右前cliff
};

class CliffData
{
public:
    CliffData() {};

    CliffData(uint8_t _cliff_id) : id_(_cliff_id) {};

    /**
     * @brief 获取二进制位
     * @return
     */
    uint8_t GetCliffBit()
    {
        FindBit();
        return id_;
    }

    /**
     * @brief 设置Bit
     * @return
     */
    void SetBit(uint8_t id)
    {
        id_ = id;
    }

    void FindBit()
    {
        for (int i = 5; i > 2; i--)
        {
            if (data_array[i] > 0)
            {
                id_ = id_map[i];
                std::cout << "Cliff Bit= " << unsigned(id_) << std::endl;
                return;
            }
        }
        for (int i = 0; i < 3; i++)
        {
            if (data_array[i] > 0)
            {
                id_ = id_map[i];
                std::cout << "Cliff Bit= " << unsigned(id_) << std::endl;
                return;
            }
        }
        id_ = 0;
    }

    void SetAllData(std::vector<u_int8_t> &data, double &CLIFF_MAXDIST)
    {
        real_time = Time::Now();
        data_array[0] = data_array[1] = data_array[2] = data_array[3] = data_array[4] = data_array[5] = 0;
        for (int i = 0; i < data.size(); i++)
        {
            if (data[i] < 1)
            {
                //std::cout<< "cliff_data = "<< data[i] <<" i= " << i<<std::endl;
                data_array[i] = CLIFF_MAXDIST;
            }
        }
    }

    void SetData(int i, double data, double &CLIFF_MAXDIST)
    {
        real_time = Time::Now();
        data_array[0] = data_array[1] = data_array[2] = data_array[3] = data_array[4] = data_array[5] = 0;
        if (data > CLIFF_MAXDIST)
        {
            //std::cout<< "cliff_data = "<< data <<" i= " << i<<std::endl;
            data_array[i] = data;
        }
        //else
        //   data_array[i]=0;
    }

    /**
     * @brief 获取ID
     * @return
     */
    //是否为右边id_ & RIGHT
    //是否为左边id_ & LEFT
    /*CliffId GetCliffId() const
    {
        if (id_ & CliffId::LEFT_FRON)
        {
            return CliffId::LEFT_FRON;
        }
        else if (id_ & CliffId::RIGH_FRON)
        {
            return CliffId::RIGH_FRON;
        }
        else if (id_ & CliffId::LEFT_BACK)
        {
            return CliffId::LEFT_BACK;
        }
        else
        {
            return CliffId::RIGH_BACK;
        }
    }*/
    CliffId GetCliffId() const
    {
        if (id_ & CliffId::LEFT_FRON) return CliffId::LEFT_FRON;
        else if (id_ & CliffId::LEFT_MIDD) return CliffId::LEFT_MIDD;
        else if (id_ & CliffId::LEFT_BACK) return CliffId::LEFT_BACK;
        else if (id_ & CliffId::RIGH_FRON) return CliffId::RIGH_FRON;
        else if (id_ & CliffId::RIGH_MIDD) return CliffId::RIGH_MIDD;
        else if (id_ & CliffId::RIGH_BACK) return CliffId::RIGH_BACK;
        else return CliffId::NO_CLIFF;
    }

    //CliffId GetCliffId() const
    //{
    //     return id_;
    //}
    void SetTime() { real_time = Time::Now(); };

    void SetTime(double _time) { real_time = _time; };

    double GetRealTime() { return real_time; };

public :
    std::map<std::string, int> cliff_map = {{std::string("base_range0"), 0},
                                            {std::string("base_range1"), 1},
                                            {std::string("base_range2"), 2},
                                            {std::string("base_range3"), 3},
                                            {std::string("base_range4"), 4},
                                            {std::string("base_range5"), 5}};
    std::map<int, uint8_t> id_map = {{0, 1},
                                     {1, 2},
                                     {2, 4},
                                     {3, 8},
                                     {4, 16},
                                     {5, 32}};

private:
    double data_array[6] = {0, 0, 0, 0, 0, 0};
    double real_time = 0;
    uint8_t id_ = 0;
};
} // namespace behavior_controller